00100	DEFINE S1="16.24",S2="6.05",S6="9.38";
00200	DEFINE S3LL="6.5",SX="20.035",SY="10.305";
00300	
00400		PRELOAD_WITH SX,SY,S1,1.0;
00500		INTERNAL SAFE REAL ARRAY SHOLDER[1:4];
00600	
00700		PRELOAD_WITH -185.0, 60.0, -185.0, -60.0, 6.5, 27.5, -175.0, 140.0, -101.0, 101.0;
00800		INTERNAL SAFE REAL ARRAY STOP[1:5,1:2];
00900	
01000	DEFINE M6="112.3", Z6="-3.09", I6XX="1411.2", I6YY="1320.6", I6ZZ="131.3";
01400	
01500	PRELOAD_WITH 400.0, 720.0, 70.0, 100.0, 160.0, 100.0;
01600	SAFE OWN REAL ARRAY F0[1:6];
01700	PRELOAD_WITH AVT(0.0, S1, 0.0, -1.0),
01800	 	AVT(0.0, S2, 0.0, 1.0), 
01900		AVS(0.0, 1.0, 0.0, 0.0, -1.0),
02000		AVT(0.0, 0.0, 0.0, -1.0), 
02100		AVT(0.0, 0.0, 0.0, 1.0), 
02200		AVT(0.0, S6, 1.0, 0.0);
02300	SAFE REAL ARRAY A[0:101];
02400	
02500	PRELOAD_WITH AVP(0.0, S1, 0.0, -1.0),
02600	 	AVP(0.0, S2, 0.0, 1.0), 
02700		AVP(0.0, 0.0, 0.0, 0.0, ),
02800		AVP(0.0, 0.0, 0.0, -1.0), 
02900		AVP(0.0, 0.0, 0.0, 1.0), 
03000		AVP(0.0, S6, 1.0, 0.0);
03100	SAFE REAL ARRAY APAR [0:35];
03200	
03300	PRELOAD_WITH QT, QS;
03400	SAFE REAL ARRAY Q[0:33];
03500	
03600	PRELOAD_WITH JDEF(1849.0, 0.0, 0.0, -1.69, 0.0, 19780.0, 0.0),
03700		JDEF(935.2, 0.0, -4.28, -1.67, 34678.5, 7365.2, 27907.9),
03800		JDEF(668.2, 0.0, 0.0, -18.1, 255446.0, 255446.0, 126.0),
03900		JDEF(365.0, 0.0, 1.323, 0.0, 2280.0, 628.0, 1820.0),
04000		JDEF(145.0, 0.0, 0.0, 2.82, 1542.0, 1450.0, 136.0),
04100		JDEF(112.3, 0.0, 0.0, -3.09, 1411.2, 1320.6, 131.3);
04200	SAFE REAL ARRAY JMAT[0:101];
04300	
04400	
04500	PRELOAD_WITH 0.18, 0.18, 0.015, 0.25, 0.25, 0.30;
04600	SAFE REAL ARRAY MA[1:6];
04700	
     

00100	SIMPLE PROCEDURE ARM_SOLVE(SAFE REAL ARRAY T,J;REFERENCE INTEGER FLAG);
00200	BEGIN	SAFE OWN REAL ARRAY VT,P,W,A,Y3,Z3,Y6,Z6X[1:4];
00300		REAL R,TH,SN1,SN2,C1,C2;
00400	LABEL NONE;
00500		INTEGER I;
00600	BOOLEAN SIMPLE PROCEDURE JOINT4(SAFE REAL ARRAY J);
00700	BEGIN	J[4]←ANGLE(VT,Y3,Z3);
00800		IF (STOP[4,1]-J[4])*(J[4]-STOP[4,2]) ≥ 0.0 THEN RETURN (TRUE);
00900		J[4]←IF J[4]≤ 0.0 THEN J[4]+360.0 ELSE J[4]-360.0;
01000		RETURN((STOP[4,1]-J[4])*(J[4]-STOP[4,2]) ≥ 0.0 );
01100	END;
01200	
01300	BOOLEAN SIMPLE PROCEDURE JOINT5(SAFE REAL ARRAY J);
01400	BEGIN	J[5]←ANGLE(Z6X,Z3,VT);
01500		RETURN((STOP[5,1]-J[5])*(J[5]-STOP[5,2]) ≥ 0.0 );
01600	END;
01700	
01800	SIMPLE PROCEDURE JOINT6(SAFE REAL ARRAY J);
01900	BEGIN	REAL R;
02000		R←ANGLE(Y6,VT,Z6X);
02100		WHILE R-J[6]>180.0 DO R←R-360.0;
02200		WHILE R-J[6]<-180.0 DO R←R+360.0;
02300		J[6]←R;
02400	END;
02500	
02600		CVV(P,T,4);
02700		CVV(Z6X,T,3);
02800		SCALE(VT,Z6X,-S6);
02900		PLUS(W,P,VT);
03000		REDUCE (W);
03100		DIFFERENCE(A,W,SHOLDER);
03200		REDUCE(A);
03300		R←DOT(A,A);
03400		R←R-S2↑2;
03500		IF R<0.0 THEN GO TO NONE;
03600		J[3]←SQRT(R);
03700		IF (STOP[3,1]-J[3])*(J[3]-STOP[3,2])< 0.0  THEN GO TO NONE;
03800		TH←ATAN2((SHOLDER[2]-W[2]),(SHOLDER[1]-W[1]));
03900		TH←IF TH>0.0 THEN TH-TPI ELSE TH;
04000		R←SQRT((A[1]↑2+A[2]↑2));
04100		IF (R←S2/R)>1.0 THEN GO TO NONE;
04200		J[1]←RAD*(TH+ASIN(R));
04300		IF (STOP[1,1]-J[1])*(J[1]-STOP[1,2])< 0.0  THEN GO TO NONE;
04400		J[2]←RAD*(-ACOS(((W[3]-SHOLDER[3])/J[3])));
04500		SN1←SIND(J[1]);
04600		C1←COSD(J[1]);
04700		SN2←SIND(J[2]);
04800		C2←COSD(J[2]);
04900		Y3[4]←Z3[4]←1.0;
05000		Y3[1]←C1*C2;
05100		Y3[2]←SN1*C2;
05200		Y3[3]←-SN2;
05300		Z3[1]←C1*SN2;
05400		Z3[2]←SN1*SN2;
05500		Z3[3]←C2;
05600		CVV(Y6,T,2);
05700		CROSS(VT,Z3,Z6X);
05800		IF MAGNITUDE(VT)<1.0@-6 THEN MOVEV(VT,Y3) ELSE UNIT(VT,VT);
05900		IF JOINT4(J)
06000		THEN	BEGIN
06100			IF FLAG←JOINT5(J)
06200			THEN JOINT6(J);
06300			RETURN END;
06400		SCALE(VT,VT,-1.0);
06500		IF FLAG←JOINT4(J)
06600		THEN	IF FLAG←JOINT5(J)
06700			THEN	JOINT6(J);
06800		RETURN;
06900	NONE:	FLAG←0;
07000	END;